#include "graph.h"

#include "util/common_macros.h"		// macros
#include "util/mmgr.h"		    
#include <cassert>
using namespace num;

Graph::Graph()
{
	m_pPather = new micropather::MicroPather(this);
}

Graph::~Graph()
{
	Reset();
}

void	Graph::Reset()
{
	for (unsigned int i=0; i<m_Nodes.size(); i++) delete m_Nodes[i];
	m_Nodes.resize(0);
	for (unsigned int i=0; i<m_Edges.size(); i++) delete m_Edges[i];
	m_Edges.resize(0);
}

void	Graph::AddNode(GRAPH_NODE_ID NodeID, const math::Vec2& Pos)
{
	GraphNode* pNode = GetNode(NodeID);
	if (pNode) 
	{
		// already exists
		return;
	}

	pNode = new GraphNode;
	pNode->ID = NodeID;
	pNode->Pos = Pos;
	m_Nodes.push_back(pNode);

}

bool	Graph::AddEdge(GRAPH_NODE_ID NodeA, GRAPH_NODE_ID NodeB)
{
	GraphEdge*	pEdge = new GraphEdge();
	pEdge->From = NodeA;
	pEdge->To = NodeB;
	m_Edges.push_back(pEdge);
	return true;
}


float	Graph::LeastCostEstimate( void* stateStart, void* stateEnd )
{
	GraphNode* pA = (GraphNode*)(stateStart);
	GraphNode* pB = (GraphNode*)(stateEnd);
	return (pA->Pos - pB->Pos).Magnitude();
}

void	Graph::AdjacentCost( void* state, std::vector< micropather::StateCost > *adjacent )
{
	GraphNode* pA = (GraphNode*)(state);
	for (unsigned int i=0; i<m_Edges.size(); i++)
	{
		GraphEdge*	pEdge = m_Edges[i];
		GraphNode* pTo = NULL;
		if (pEdge->From==pA->ID)
		{
			pTo = GetNode(pEdge->To);
		} else
		if (pEdge->To == pA->ID)
		{
			pTo = GetNode(pEdge->From);
		}
		if (pTo)
		{
			micropather::StateCost	Cost;
			Cost.state = pTo;

			// distance?
			Cost.cost	= (pA->Pos - pTo->Pos).Magnitude();
			// add to set
			adjacent->push_back(Cost);
		}
	}
	
}

Graph::GraphNode*	Graph::GetNode(GRAPH_NODE_ID ID)
{
	for (unsigned int i=0; i<m_Nodes.size(); i++)
		if (m_Nodes[i]->ID==ID) return m_Nodes[i];
	return NULL;
}

bool	Graph::FindPath(GRAPH_NODE_ID From, GRAPH_NODE_ID To, std::vector<GRAPH_NODE_ID>& Path)
{
	GraphNode*	pFrom	= GetNode(From);
	GraphNode*	pTo		= GetNode(To);
	if (!pFrom || !pTo) return false;

	void*	pPartial = NULL;
	int Res = m_pPather->Solve(pFrom, pTo, &m_Path, &m_TotalCost, pPartial); 
	if (Res == micropather::MicroPather::NO_SOLUTION) return false;

	// copy it over
	//for (int i=int(m_Path.size())-1; i>=0; i--)
	for (unsigned int i=0; i<m_Path.size(); i++)
	{
		GraphNode*	pNode = (GraphNode*)m_Path[i];
		Path.push_back(pNode->ID);
	}
	return true;
}
